#include "controller.h"
#include "time_source.h"
#include "logical_clock.h"
#include <vector>
#include <random>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <chrono>
#include <iomanip>
#include <thread>
#include "synth.h"
#include "synth_matlab_bridge.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>


struct ControllerConfig;

// struct Node {
//     Controller ctrl;
//     double x_ns{0.0};       // 逻辑时钟
//     double drift_ppm{0.0};  // 基准漂移
//     int left{-1}, right{-1};
//     double wL{1.0}, wR{1.0};
//     Node(const ControllerConfig& c): ctrl(c) {}
// };

static void sleep_ms(int ms){
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

// 打印工具
static void print_tf(const TF& H, const std::string& name) {
    std::cout << name << " num: ";
    for (size_t i=0;i<H.num.size();++i) { if(i) std::cout<<", "; std::cout<<H.num[i]; }
    std::cout << "\n" << name << " den: ";
    for (size_t i=0;i<H.den.size();++i) { if(i) std::cout<<", "; std::cout<<H.den[i]; }
    std::cout << "\n";
}

// ---- Node name resolution (CLI > ENV > compile-time macro) ----
#ifndef TSYNC_NODE_NAME
#define TSYNC_NODE_NAME ""   // 可在编译期用 -DTSYNC_NODE_NAME="50010" 赋值
#endif

static std::string resolve_node_name(const char* cli_name) {
    if (cli_name && *cli_name) return std::string(cli_name);

    if (const char* env = std::getenv("TSYNC_NODE_NAME")) {
        if (*env) return std::string(env);
    }
    if (std::strlen(TSYNC_NODE_NAME) > 0) return std::string(TSYNC_NODE_NAME);
    return std::string(); // 空：未指定
}



// --- 工具: 漂亮打印状态空间矩阵 ---
static void print_statespace(const std::string& title, const StateSpace& S) {
    using std::cout;
    auto fmt = Eigen::IOFormat(10, 0, ", ", ";\n", "[ ", " ]");

    cout << "\n=== " << title << " ===\n";
    if (S.A.size() == 0) {
        // D 可能是 1x1 或空
        if (S.D.size() == 1) {
            cout << "Static gain (no states)\n";
            cout << "D = " << std::setprecision(17) << S.D(0,0) << "\n";
        } else {
            cout << "Static gain (no states)\n";
            cout << "D = \n" << S.D.format(fmt) << "\n";
        }
        return;
    }

    cout << "Order n = " << S.A.rows() << "\n";
    cout << "A = \n" << S.A.format(fmt) << "\n";
    if (S.B.size()) cout << "B = \n" << S.B.format(fmt) << "\n";   // 支持 n×1 或 n×m
    if (S.C.size()) cout << "C = \n" << S.C.format(fmt) << "\n";   // 支持 p×n
    if (S.D.size()==1) {
        cout << "D = " << std::setprecision(17) << S.D(0,0) << "\n";
    } else {
        cout << "D = \n" << S.D.format(fmt) << "\n";
    }

    Eigen::VectorXcd evals = S.A.cast<std::complex<double>>().eigenvalues();
    cout << "poles |λ| = ";
    for (int i = 0; i < evals.size(); ++i) {
        cout << std::abs(evals(i)) << (i + 1 < evals.size() ? ", " : "");
    }
    cout << "\n";
}

// --- 工具: 缩放传递函数（把 num 全部乘以 scale；den 不变）---
static TF tf_scale_num(const TF& H, double scale) {
    TF out = H;
    for (auto& b : out.num) b *= scale;
    return out;
}

// 构造 n 节点环形图的拉普拉斯矩阵（对称）
static std::vector<std::vector<double>> ring_laplacian(int n) {
    std::vector<std::vector<double>> A(n, std::vector<double>(n, 0.0));
    for (int i = 0; i < n; ++i) {
        int j1 = (i - 1 + n) % n;
        int j2 = (i + 1) % n;
        A[i][j1] = A[j1][i] = 1.0;
        if (j2 != j1) { // n=2 时避免重复
            A[i][j2] = A[j2][i] = 1.0;
        }
    }
    // L = D - A
    std::vector<std::vector<double>> L(n, std::vector<double>(n, 0.0));
    for (int i = 0; i < n; ++i) {
        double deg = 0.0;
        for (int j = 0; j < n; ++j) deg += A[i][j];
        L[i][i] = deg;
        for (int j = 0; j < n; ++j) if (i != j) L[i][j] = -A[i][j];
    }
    return L;
}


Eigen::MatrixXd custom_laplacian() {
    int n = 6;
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, n);

    // === 根据图中的边添加邻接关系 ===
    // (1–2), (1–4), (2–3), (2–4), (2–5), (3–6), (4–6), (5–6)
    std::vector<std::pair<int,int>> edges = {
        {0,1}, {0,3}, {1,2}, {1,3}, {1,4}, {2,5}, {3,5}, {4,5}
    };

    for (auto [u,v] : edges) {
        A(u,v) = 1;
        A(v,u) = 1;
    }

    // 度矩阵 D
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(n, n);
    for (int i = 0; i < n; i++) {
        D(i,i) = A.row(i).sum();
    }

    // 拉普拉斯矩阵 L = D - A
    return D - A;
}

// 求特征值
std::vector<double> laplacian_eigs(const Eigen::MatrixXd& L) {
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(L);
    Eigen::VectorXd eigvals = solver.eigenvalues();
    std::vector<double> res(eigvals.data(), eigvals.data() + eigvals.size());
    return res;
}

extern StateSpace tf_to_ss_proper(const TF&);


static void usage(const char* prog){
    std::fprintf(stderr,
      "Usage: %s --bind <IP> [--tau-ns <ns>=50000000] [--recv-window-ms <ms>=8] "
      "[--listen-port <udp>=5001] [--k-limit <float>=0.02] [--name <node_id>]\n"
      "Notes:\n"
      "  - Controller config files:\n"
      "      kz : data/kz.json\n"
      "      neighbors : ../data/neighbors.json (or TSYNC_NEIGHBORS env)\n"
      "  - Node name precedence: --name > env TSYNC_NODE_NAME > -DTSYNC_NODE_NAME\n"
      "  - Example: %s --bind 11.11.11.11 --name 50010 --k-limit 0.05\n",
      prog, prog);
}

int main(int argc, char** argv){
    //1+++++++++++++++++++++
    // int N=8, tau_ms=200;
    // ControllerConfig cfg;
    // std::vector<Node> nodes; nodes.reserve(N);
    // for(int i=0;i<N;i++) nodes.emplace_back(cfg);
    // for(int i=0;i<N;i++){ nodes[i].left=(i-1+N)%N; nodes[i].right=(i+1)%N; }
    //
    // std::mt19937_64 rng(123);
    // std::uniform_real_distribution<double> ppm(-50.0,50.0);
    // for(auto& n:nodes) n.drift_ppm = ppm(rng);
    //
    // for(int k=0;k<300;k++){
    //     // 误差 ẽ
    //     std::vector<double> e(N,0.0);
    //     for(int i=0;i<N;i++){
    //         e[i] = nodes[i].wL*(nodes[i].x_ns - nodes[nodes[i].left].x_ns)
    //              + nodes[i].wR*(nodes[i].x_ns - nodes[nodes[i].right].x_ns);
    //     }
    //     // 控制器输出并更新时钟
    //     for(int i=0;i<N;i++){
    //         double u_ppm = nodes[i].ctrl.update(e[i]); // 建议调整
    //         double total_ppm = nodes[i].drift_ppm - u_ppm; // 用调整抵消漂移
    //         nodes[i].x_ns += (double)tau_ms * 1e6 * (1.0 + total_ppm*1e-6);
    //     }
    //     if(k%10==0){
    //         double mean=0; for(auto& n:nodes) mean+=n.x_ns; mean/=N;
    //         std::vector<double> dev; dev.reserve(N);
    //         for(auto& n:nodes) dev.push_back(std::abs(n.x_ns-mean));
    //         std::sort(dev.begin(),dev.end());
    //         double avg = std::accumulate(dev.begin(),dev.end(),0.0)/N;
    //         double p99 = dev[(size_t)(0.99*(N-1))];
    //         std::cout<<"k="<<k<<" A-Avg(ns)="<<(long long)avg<<" CI99(ns)="<<(long long)p99<<"\n";
    //     }
    //     sleep_ms(tau_ms/4);
    // }
    // return 0;

    //2+++++++++++++++++++++
    //run_tsc_demo(15, 200);

    //3+++++++++++++++++++++
    //初始化 TSC（内部会做频率校准）
    // auto calib = TscSource::init(0.3);
    // if (!calib.ok) { std::cerr<<"No invariant TSC\n"; return 1; }
    //
    // LogicalClock clk(/*init_offset_ns=*/0);
    //
    // // 对外取时间
    // int64_t t_logic = clk.now_ns();
    // std::cout << "logic_now(ns)=" << t_logic << "\n";
    //
    // // 来了一次邻居对时偏差估计 offset = -850ns，做一次快速校正（50%）
    // clk.apply_phase_error(/*e_ns=*/-850, /*alpha=*/0.5, /*slew_rate=*/0);
    //
    // // 或者慢慢拉回（每秒最多调整 0.2ms）
    // clk.slew_towards(/*target_off*/ 10'000, /*slew_rate_ns_per_s=*/200'000);
    //
    // // 取节拍相位（tau=100ms）
    // uint32_t phi = clk.phase_mod_tau(100'000'000);
    // std::cout << "phase(ns mod 100ms)=" << phi << "\n";

    //4++++++++++++++++++++
    // -------- 1) 测试参数 --------
  // double tau = 0.1;                             // 采样周期
  // std::vector<double> lambdas = {2.0, 4.0, 6.0}; // 拉普拉斯非零特征值
  // double g_lo = 1.01, g_hi = 100.0;             // γ 搜索区间
  //
  // // 选择一个 \tilde k(z) 原型（proper TF，a0=1）
  // // 例1：常数增益 \tilde k(z) = 0.2
  // TF ktilde_proto;
  // ktilde_proto.num = {0.2};   // b0
  // ktilde_proto.den = {1.0};   // a0=1
  //
  // // 也可以换成一阶滞后（请保证 proper）：\tilde k(z) = 0.15 / (1 - 0.7 z^{-1})
  // // ktilde_proto.num = {0.15};
  // // ktilde_proto.den = {1.0, -0.7};
  //
  // // -------- 2) 求解控制器并输出 --------
  // SynthResult R = synth_controller(lambdas, tau, ktilde_proto, g_lo, g_hi);
  //
  // std::cout << std::fixed << std::setprecision(6);
  // std::cout << "gamma* = " << R.gamma_star << "\n\n";
  //
  // print_tf(R.ktilde, "tilde k(z)");
  // print_tf(R.k,      "final k(z) = sqrt(gamma*) * tilde k(z)");
  //
  // // 打印状态空间（供你塞进控制器类）
  // int n = (int)std::sqrt((double)R.A.size());
  // std::cout << "\nState-space of k(z):\nA = \n";
  // for (int i=0;i<n;i++) {
  //   for (int j=0;j<n;j++) std::cout << std::setw(12) << R.A[i*n+j] << " ";
  //   std::cout << "\n";
  // }
  // std::cout << "B^T = [ ";
  // for (int i=0;i<n;i++) std::cout << R.B[i] << " ";
  // std::cout << "]\nC = [ ";
  // for (int i=0;i<n;i++) std::cout << R.C[i] << " ";
  // std::cout << "]\nD = " << R.D << "\n\n";
  //
  // // -------- 3) 可行性核验：计算 ||\tilde M_i||_∞ --------
  // bool all_ok = true;
  // for (double lam : lambdas) {
  //   PaperParams P = paper_params(lam, tau, R.gamma_star);
  //   StateSpace Mi = Mi_from_params(P, lam, R.ktilde);   // 注意验证用的是 \tilde k(z)
  //   double ninf = hinf_norm(Mi, /*worN=*/2048, 1.0 - 1e-9);
  //   std::cout << "||tilde M_i||_inf for lambda=" << lam << " : " << ninf << "\n";
  //   if (ninf >= 1.0 - 1e-9) all_ok = false;
  // }
  // std::cout << "\nFeasibility check: " << (all_ok ? "PASS (<1 for all i)" : "FAIL") << "\n";
  //
  // // -------- 4) 额外：频响看看 \tilde k(e^{jw}) 的幅值 --------
  // std::cout << "\n|tilde k(e^{jw})| at a few w:\n";
  // for (double w : {0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0}) {
  //   std::complex<double> Kw = tf_eval(R.ktilde, w);
  //   std::cout << "  w=" << w << "  |K|=" << std::abs(Kw) << "\n";
  // }

    //5+++++++++++++++++++++++++
    // using std::cout;
    // using std::endl;
    //
    // // === 1) 配置路径 ===
    // // 你已经验证 E:\\matlab\\bin\\matlab.exe 可以运行 -batch
    // const std::string matlabExe = "E:\\matlab\\bin\\matlab.exe";
    // // .m 文件目录：看你截图是 D:\\timesync\\matlab
    // const std::string mfileDir  = "D:\\timesync\\matlab";
    //
    // // === 2) 论文参数（随便给一组可运行值） ===
    // const double tau        = 0.2;   // s：采样周期
    // const double lambda_max = 5.0;   // 最大特征值（示例）
    // const double gamma_try  = 100.0;   // 先试一个 γ
    //
    // cout << "Calling MATLAB to synthesize k~(z) with:\n"
    //      << "  tau=" << tau << ", lambda_max=" << lambda_max
    //      << ", gamma=" << gamma_try << "\n";
    //
    // TF ktilde;
    // std::string err;
    // bool ok = tsync::matlab_synthesize_ktilde(
    //     matlabExe, mfileDir, tau, lambda_max, gamma_try, ktilde, &err);
    //
    // if (!ok) {
    //     std::cerr << "MATLAB synthesis FAILED: " << err << "\n";
    //     return 1;
    // }
    //
    // // === 3) 打印传递函数系数 ===
    // cout << "ktilde.num = [ ";
    // for (size_t i=0;i<ktilde.num.size();++i) {
    //     if (i) cout << ", ";
    //     cout << std::setprecision(17) << ktilde.num[i];
    // }
    // cout << " ]\n";
    //
    // cout << "ktilde.den = [ ";
    // for (size_t i=0;i<ktilde.den.size();++i) {
    //     if (i) cout << ", ";
    //     cout << std::setprecision(17) << ktilde.den[i];
    // }
    // cout << " ]\n";
    //
    // // === 4) 把 k~(z) 转成状态空间并打印极点模长（自检） ===
    // try {
    //     StateSpace S = tf_to_ss_proper(ktilde);
    //     if (S.A.size() > 0) {
    //         Eigen::VectorXcd evals = S.A.cast<std::complex<double>>().eigenvalues();
    //         cout << "poles |λ| = ";
    //         for (int i=0;i<evals.size(); ++i) {
    //             cout << std::abs(evals(i)) << (i+1<evals.size() ? ", " : "");
    //         }
    //         cout << "\n";
    //     } else {
    //         cout << "k~(z) is static (no states).\n";
    //     }
    // } catch (const std::exception& e) {
    //     std::cerr << "tf_to_ss_proper error: " << e.what() << "\n";
    // }
    //
    // cout << "OK.\n";
    //
    // // === 4) 把 k~(z) 转成状态空间并打印 A/B/C/D 与极点模长 ===
    // try {
    //     StateSpace S = tf_to_ss_proper(ktilde);
    //
    //     if (S.A.size() > 0) {
    //         print_statespace("State-space of k~(z)", S);
    //     } else {
    //         std::cout << "k~(z) is static (no states).\n";
    //         print_statespace("State-space of k~(z)", S);
    //     }
    // } catch (const std::exception& e) {
    //     std::cerr << "tf_to_ss_proper(k~) error: " << e.what() << "\n";
    // }
    //
    // // === 5) 若需要，同时打印最终控制器 k(z) = sqrt(gamma) * k~(z) ===
    // {
    //     const double gamma = gamma_try;            // 你这里先用试探的 gamma
    //     const double scale = std::sqrt(gamma);     // k(z) = scale * ktilde(z)
    //     TF k = tf_scale_num(ktilde, scale);
    //
    //     // 打印 k(z) 的 num/den
    //     cout << "k.num = [ ";
    //     for (size_t i=0;i<k.num.size();++i) {
    //         if (i) cout << ", ";
    //         cout << std::setprecision(17) << k.num[i];
    //     }
    //     cout << " ]\n";
    //
    //     cout << "k.den = [ ";
    //     for (size_t i=0;i<k.den.size();++i) {
    //         if (i) cout << ", ";
    //         cout << std::setprecision(17) << k.den[i];
    //     }
    //     cout << " ]\n";
    //
    //     try {
    //         StateSpace S2 = tf_to_ss_proper(k);
    //         print_statespace("State-space of k(z)", S2);
    //     } catch (const std::exception& e) {
    //         std::cerr << "tf_to_ss_proper(k) error: " << e.what() << "\n";
    //     }
    // }

    //6+++++++++++++++++++++++++++++++++++++++++++++
    //6+++++++++++++++++++++++++++++++++++++++++++++
    //6+++++++++++++++++++++++++++++++++++++++++++++
// try {
//     using std::cout;
//     using std::endl;
//     std::cout << std::fixed << std::setprecision(6);
//
//     // === 1) 三节点「星型图 Star(3)」的拉普拉斯矩阵 ===
//     const int n = 3;
//     const int center = 1; // 修改：以节点2为中心（索引1对应节点2）
//     Eigen::MatrixXd L = Eigen::MatrixXd::Zero(n, n);
//
//     // 单位权星型：中心2（索引1）与节点1（索引0）、节点3（索引2）相连，权重 w = 1
//     double deg_center = 0.0;
//     for (int i = 0; i < n; ++i) {
//         if (i == center) continue;
//         const double w = 1.0;             // 如需加权星型，把这里改成不同的 w_i
//         L(center, i) = L(i, center) = -w; // L = D - A
//         L(i, i) += w;                     // 叶子度数
//         deg_center += w;                  // 累计中心度数
//     }
//     L(center, center) += deg_center;
//
//     // （可选）校验：Star(3) 的特征值仍为 {0, 1, 3}（拓扑同构，特征值不变）
//
//     // === 2) 求拉普拉斯非零特征值 ===
//     auto eigs = laplacian_eigs(L);
//     std::vector<double> lambdas;
//     lambdas.reserve(eigs.size());
//     for (double v : eigs) {
//         if (std::abs(v) > 1e-9) lambdas.push_back(v); // 去掉 0 特征值
//     }
//     std::sort(lambdas.begin(), lambdas.end());
//
//     // 打印看一下：应为 1 和 3（各出现 1 次）
//     std::cout << "[synth] Star(3) Laplacian nonzero lambdas:";
//     for (double v : lambdas) std::cout << " " << v;
//     std::cout << "\n";
//
//     // === 3) 选择 tau 与原型 ktilde，然后综合求解 gamma* 与最终 k(z) ===
//     const double tau = 0.2; // 采样周期(秒)，按你的系统设定
//     TF ktilde_proto;
//     // 举例：Ktilde(z) proper 型（保持你的原设计）
//     ktilde_proto.num = { 0.05, 0.01 };
//     ktilde_proto.den = { 1.0, -0.8 };
//
//     // 搜索 gamma 并生成最终控制器 k(z) = sqrt(gamma*) * ktilde
//     const double g_lo = 1.0, g_hi = 200.0;
//     SynthResult R = synth_controller(lambdas, tau, ktilde_proto, g_lo, g_hi);
//
//     std::cout << "gamma* = " << R.gamma_star << "\n\n";
//     print_tf(R.ktilde, "tilde k(z)");
//     print_tf(R.k,      "final k(z) = sqrt(gamma*) * tilde k(z)");
//
//     // （可选）状态空间自检
//     try {
//         StateSpace S = tf_to_ss_proper(R.k);
//         print_statespace("State-space of k(z)", S);
//     } catch (const std::exception& e) {
//         std::cerr << "[warn] tf_to_ss_proper(k) failed: " << e.what() << "\n";
//     }
//
//     // === 4) 写文件（沿用你现有的路径）===
//     if (!save_kz_json(R.k, "../data/kz.json", /*gamma=*/R.gamma_star)) {
//         std::cerr << "[synth] warning: save ../data/kz.json failed\n";
//     } else {
//         std::cout << "[synth] wrote ../data/kz.json\n";
//     }
//     if (!save_kz_json(R.ktilde, "data/ktilde.json", /*gamma=*/R.gamma_star)) {
//         std::cerr << "[synth] warning: save data/ktilde.json failed\n";
//     } else {
//         std::cout << "[synth] wrote data/ktilde.json\n";
//     }
//
// } catch (const std::exception& e) {
//     std::cerr << "[synth] ERROR: " << e.what() << "\n";
//     return 1;
// } catch (...) {
//     std::cerr << "[synth] ERROR: unknown exception\n";
//     return 1;
// }


    // 7+++++++++++++++++++++++++++++++++++++++++++++++++
    const char* bind_ip = nullptr;        // 监听所有本地网卡
    uint32_t    tau_ns = 200'000'000;     // 0.2 s = 200 ms
    int         recv_window_ms = 8;       // 仍为 800 ms（需要可再调大）
    uint16_t    listen_port = 5001;       // 默认开启 responder
    double      k_limit = 0.02;           // 新增：控制器增益缩放（运行时传入）


    const char* cli_node_name = nullptr;

    for (int i=1;i<argc;i++){
        if (!std::strcmp(argv[i],"--bind") && i+1<argc) { bind_ip = argv[++i]; }
        else if (!std::strcmp(argv[i],"--tau-ns") && i+1<argc) { tau_ns = (uint32_t)std::strtoul(argv[++i],nullptr,10); }
        else if (!std::strcmp(argv[i],"--recv-window-ms") && i+1<argc) { recv_window_ms = std::atoi(argv[++i]); }
        else if (!std::strcmp(argv[i],"--listen-port") && i+1<argc) { listen_port = (uint16_t)std::strtoul(argv[++i],nullptr,10); }
        else if (!std::strcmp(argv[i],"--k-limit") && i+1<argc) { k_limit = std::atof(argv[++i]); }
        else if (!std::strcmp(argv[i],"--name") && i+1<argc) { cli_node_name = argv[++i]; }
        else { usage(argv[0]); return 1; }
    }
    if (!bind_ip){ usage(argv[0]); return 1; }

    // 解析节点名
    const std::string node_name = resolve_node_name(cli_node_name);
    if (node_name.empty()) {
        std::fprintf(stderr, "[WARN] node name not specified; metrics will be written to 'metrics.csv'\n");
    }

    // 输出目录（你已有 gdir 就用你的；没有就置为当前目录）
    // 构造文件名
    const std::string metrics_path = "metrics" + (node_name.empty() ? std::string("") : node_name) + ".csv";


    // 原来：tsync::LogicalClock lc(/*init_offset_ns=*/0);
    tsync::LogicalClock lc(/*init_offset_ns=*/0, /*enable_zero_base=*/false);
    std::vector<tsync::Neighbor> neighbors; // 此处内容会在 requester_loop 内被文件加载覆盖
    try {
        tsync::run_controller(neighbors, tau_ns, bind_ip, recv_window_ms, listen_port, k_limit,lc,metrics_path);
    } catch (const std::exception& e) {
        std::fprintf(stderr, "[FATAL] %s\n", e.what());
        return 2;
    }
    return 0;


    return 0;
}
